#include <cstdlib>
#include <stdio.h>
#include "node.h"
#define ABS(a) ((a)<0?-(a):a)
#define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define MAX(a, b)  (((a) > (b)) ? (a) : (b))

#include  "math.h"
#define ROBOTID 0
#define FIELD_X 15
#define FIELD_Y 15
#define TRANSPONEERD 10

int main( int argc, char* argv[])
{


    //variabelen
    int i,j;
    int field[FIELD_X][FIELD_Y];
    int i_start,i_end;
    int j_start,j_end;

    int wanted_x,wanted_y,wanted_rotation;
    int own_x,own_y,own_rotation;

    //coordinaten in lokale vars steken
    wanted_x = atoi( argv[1]);
    wanted_y = atoi( argv[2]);
    wanted_rotation = atoi( argv[3]);

    own_x = atoi(argv[4]);
    own_y = atoi(argv[5]);
    own_rotation = atoi(argv[6]);


    //Filling the simplefied field with 99
    for(i=0; i<FIELD_X; i++)
    {
        for(j=0; j<FIELD_X; j++)
        {
            field[i][j] = 999;
        }
    }



    //calculation the upper left corner and lower right corner
    // upper_left = (i_start,j_start) lower right = (i_end,j_end)

    i_start = MIN(own_x,wanted_x);
    j_start = MIN(wanted_y,own_y);

    i_end = MAX(own_x,wanted_x);
    j_end = MAX(wanted_y,own_y);


    //Rechthoek arround the wanted and own location
    for(i=i_start; i<=i_end; i++)
    {
        for(j=j_start; j<=j_end; j++)
        {
            field[i][j] = 10;
        }
    }
#include <cstdlib>
#include <stdio.h>
#include "node.h"
#define ABS(a) ((a)<0?-(a):a)
#define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define MAX(a, b)  (((a) > (b)) ? (a) : (b))

#include  "math.h"
#define ROBOTID 0
#define FIELD_X 15
#define FIELD_Y 15
#define TRANSPONEERD 10

int main( int argc, char* argv[])
{



    //calculation the upper left corner and lower right corner
    // upper_left = (i_start,j_start) lower right = (i_end,j_end)

    i_start = MIN(own_x,wanted_x);
    j_start = MIN(wanted_y,own_y);

    i_end = MAX(own_x,wanted_x);
    j_end = MAX(wanted_y,own_y);


    //The next blob of code gives a cost value arround the wanted_location
    //The code is designed to handle edge cases and give a good angle

    //checking if the upper left corner of the "rechthoek" is an wanted location
    if( wanted_x == i_start && wanted_y == j_start )
    {
	if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
	{
	    field[wanted_x+1][wanted_y+1] = 10*( ABS(315 - wanted_rotation)%180 ) /45+1;          //cross right down
	    if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
		field[wanted_x+2][wanted_y+2] = field[wanted_x+1][wanted_y+1]*field[wanted_x+1][wanted_y+1];
	}
#include <cstdlib>
#include <stdio.h>
#include "node.h"
#define ABS(a) ((a)<0?-(a):a)
#define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define MAX(a, b)  (((a) > (b)) ? (a) : (b))

#include  "math.h"
#define ROBOTID 0
#define FIELD_X 15
#define FIELD_Y 15
#define TRANSPONEERD 10

int main( int argc, char* argv[])
{
        if( (own_x - wanted_x) != 0)
	{
            field[wanted_x+1][wanted_y] =10* (ABS(270 - wanted_rotation)%180)/45+1;           //downward
	    if( ABS(own_x - wanted_x) >= 3)
	    {
		field[wanted_x+2][wanted_y-0] = field[wanted_x+1][wanted_y-0]*field[wanted_x+1][wanted_y-0];
	  	field[wanted_x+2][wanted_y+1] = (field[wanted_x+2][wanted_y+2] + field[wanted_x+2][wanted_y-0])/2+1;
	    }
	}
        if( (own_y - wanted_y) != 0)
	{

	    field[wanted_x-0][wanted_y+1] = 10*(ABS(0 - wanted_rotation)%180)/45+1;          //right sideways
	    if( ABS(own_y - wanted_y) >= 3)
	    {
		field[wanted_x-0][wanted_y+2] = 10*field[wanted_x-0][wanted_y+1]*field[wanted_x-0][wanted_y+1];
		field[wanted_x+1][wanted_y+2] = (field[wanted_x+2][wanted_y+2] + field[wanted_x-0][wanted_y+2])/2+1;
	    }
	}

    }
    //cheking if the lower left corner of the "rechthoek" is an wanted location
    else if( wanted_x == i_end && wanted_y == j_start)
    {
	if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
	{
	    field[wanted_x-1][wanted_y+1] = 10*( ABS(135 - wanted_rotation)%180 ) /45+1;          //cross right up
	    if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
		field[wanted_x-2][wanted_y+2] = field[wanted_x-1][wanted_y+1]*field[wanted_x-1][wanted_y+1];
	}

        if( (own_x - wanted_x) != 0)
	{
            field[wanted_x-1][wanted_y-0] = 10*(ABS(90 - wanted_rotation)%180)/45+1;           //upward
	    if( ABS(own_x - wanted_x) >= 3)
	    {
		field[wanted_x-2][wanted_y-0] = field[wanted_x-1][wanted_y-0]*field[wanted_x-1][wanted_y-0];
	  	field[wanted_x-2][wanted_y+1] = (field[wanted_x-2][wanted_y+2] + field[wanted_x-2][wanted_y-0])/2+2;
	    }
	}
        if( (own_y - wanted_y) != 0)
	{

	    field[wanted_x-0][wanted_y+1] = 10*(ABS(180 - wanted_rotation)%180)/45+1;          //right sideways
	    if( ABS(own_y - wanted_y) >= 3)
	    {
		field[wanted_x-0][wanted_y+2] = field[wanted_x-0][wanted_y+1]*field[wanted_x-0][wanted_y+1];
		field[wanted_x-1][wanted_y+2] = (field[wanted_x-2][wanted_y+2] + field[wanted_x-0][wanted_y+2])/2+2;
	    }
	}
    }
    //cheking if the upper right corner of the "rechthoek" is an wanted location
    else if( wanted_x == i_start && wanted_y == j_end)
    {


        if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
	{
	    field[wanted_x+1][wanted_y-1] = 10*( ABS(225 - wanted_rotation)%180 ) /45+1;          //cross left down
	    if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
		field[wanted_x+2][wanted_y-2] = field[wanted_x+1][wanted_y-1]*field[wanted_x+1][wanted_y-1];
	}

        if( (own_x - wanted_x) != 0)
	{
            field[wanted_x+1][wanted_y-0] = 10*(ABS(270 - wanted_rotation)%180)/45+1;           //downard
	    if( ABS(own_x - wanted_x) >= 3)
	    {
		field[wanted_x+2][wanted_y-0] = field[wanted_x+1][wanted_y-0]*field[wanted_x+1][wanted_y-0];
	  	field[wanted_x+2][wanted_y-1] = (field[wanted_x+2][wanted_y-2] + field[wanted_x+2][wanted_y-0])/2+1;
	    }
	}
        if( (own_y - wanted_y) != 0)
	{

	    field[wanted_x-0][wanted_y-1] = 10*(ABS(180 - wanted_rotation)%180)/45+1;          //left sideways
	    if( ABS(own_y - wanted_y) >= 3)
	    {
		field[wanted_x-0][wanted_y-2] = field[wanted_x-0][wanted_y-1]*field[wanted_x-0][wanted_y-1];
		field[wanted_x+1][wanted_y-2] = (field[wanted_x+2][wanted_y-2] + field[wanted_x][wanted_y-2])/2+1;
	    }
	}

    }
    //cheking id the lower right corner of the "rechthoek" is an wanted location
    else if ( wanted_x == i_end && wanted_y == j_end)
    {
	if( (own_x - wanted_x) != 0 && (own_y - wanted_y) != 0)
	{
	    field[wanted_x-1][wanted_y-1] = 10*( ABS(135 - wanted_rotation)%180 ) /45+1;          //cross left up
	    if( ABS(own_x - wanted_x) >= 3 && ABS(own_y - wanted_y) >= 3 )
		field[wanted_x-2][wanted_y-2] = field[wanted_x-1][wanted_y-1]*field[wanted_x-1][wanted_y-1];
	}

        if( (own_x - wanted_x) != 0)
	{
            field[wanted_x-1][wanted_y-0] = 10*(ABS(90 - wanted_rotation)%180)/45+1;           //upward
	    if( ABS(own_x - wanted_x) >= 3)
	    {
		field[wanted_x-2][wanted_y-0] = field[wanted_x-1][wanted_y-0]*field[wanted_x-1][wanted_y-0];
	  	field[wanted_x-2][wanted_y-1] = (field[wanted_x-2][wanted_y-2] + field[wanted_x-2][wanted_y])/2+1;
	    }
	}
        if( (own_y - wanted_y) != 0)
	{

	    field[wanted_x-0][wanted_y-1] = 10*(ABS(180 - wanted_rotation)%180)/45+1;          //left sideways
	    if( ABS(own_y - wanted_y) >= 3)
	    {
		field[wanted_x-0][wanted_y-2] = field[wanted_x-0][wanted_y-1]*field[wanted_x-0][wanted_y-1];
		field[wanted_x-1][wanted_y-2] = (field[wanted_x-2][wanted_y-2] + field[wanted_x][wanted_y-2])/2+1;
	    }
	}

    }
	

    //Give robot location value 0 and wanted location value 0
    //Good for testing this function out
    field[own_x][own_y] = 0;
    field[wanted_x][wanted_y] = 0 ;



    //create large matrix
    //fill it with pointers to the newly created nodes
    //in a next step you can link them together
    Node *field_pointers[FIELD_X][FIELD_Y];
    for(i=0; i<FIELD_X; i++)
    {
        for(j=0; j<FIELD_X; j++)
        {
            field_pointers[i][j] = NULL;
        }
    }


    //create new nodes for the working rectangle
    for(i=i_start;i<=i_end;i++)
    {
        for(j=j_start;j<=j_end;j++)
        {
            field_pointers[i][j] = new Node(i,j,field[i][j]);
        }
    }

    


    //link the nodes in the working rectangle togheter in all directions: left, right, up, ...
    for(i=i_start;i<=i_end;i++)
    {
        for(j=j_start;j<=j_end;j++)
        {
            //up
            if( field[i-1][j] != 999 && i != i_start )
            {
                field_pointers[i][j]->up = field_pointers[i-1][j];
            }

            //down
            if( field[i+1][j] != 999 && i != i_end)
            {
                field_pointers[i][j]->down = field_pointers[i+1][j];
            }

            //right
            if( field[i][j+1] != 999 && j != j_end)
            {
                field_pointers[i][j]->right = field_pointers[i][j+1];
            }

            //left
            if( field[i][j-1] != 999 && j != j_start )
            {
                field_pointers[i][j]->left = field_pointers[i][j-1];
            }

            //left up
            if( field[i-1][j-1] != 999 && i != i_start && j != j_start)
            {
                field_pointers[i][j]->left_up = field_pointers[i-1][j-1];
            }

            //left down
            if( field[i+1][j-1] != 999 && i != i_end && j != j_start)
            {
                field_pointers[i][j]->left_down = field_pointers[i+1][j-1];
            }


            //right down
            if( field[i+1][j+1] != 999 && i != i_end && j != j_end)
            {
                field_pointers[i][j]->right_down = field_pointers[i+1][j+1];
            }

             //right up
            if( field[i-1][j+1] != 999 && i != i_start && j != j_end)
            {
                field_pointers[i][j]->right_up = field_pointers[i-1][j+1];
            }
        }
    }








//dijkstra algorimte
    int lowcost; 
    int open_index=0;
    int current_index;
    int flag;
    
    Node *currentN;
    Node *tempN;

    Node *open_list[FIELD_X*FIELD_Y];

    for(i=0;i<FIELD_X*FIELD_Y;i++)
    {
	open_list[i] = NULL;
    }


    //step1
    //Add the starting node to the open list. 
    currentN = field_pointers[own_x][own_y];
    currentN->cost = 0;
    currentN->open = 1;
    currentN->prev = NULL;
    open_list[open_index]=currentN;
    open_index++;

    //step 2
    //repeat the following
    while(1)
    {

	//step 2.a
	//Look for the lowest cost node on the open list. 
	//We refer to this as the current node.
	lowcost = 999;
	flag = 0;
	for(i=0;i<open_index;i++)
	{
		if( open_list[i] != NULL )
		{
			if( lowcost > open_list[i]->cost )
			{
				lowcost = open_list[i]->cost;
				current_index = i;
				flag = 1;
			}
		}
	}

	//Step 5
        //stop if there are no more open nodes
	//The shortest path from each node to the origin can be found using the backtracking prev pointer.
	if(currentN->co_x == wanted_x && currentN->co_y == wanted_y)
		break;
	if( flag == 0 )
	{
		break;
	}
	currentN = open_list[current_index];


	//step 2.b
 	//set current node as visited and remove from open list
	open_list[current_index] = NULL;
	currentN->visited = 1;
	currentN->open = 0;


	//step2.c
	//For each of the 8 nodes adjacent to this current square
	//If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.           
    	//If it isn’t on the open list, add it to the open list. Make the current node the parent of this square. Record the cost. 
    	//If it is on the open list already, check to see if recent calculated cost is lower.If so, change the parent of the node
	//to the current square, and overwrite with the lower cost.
	//add an extra cost of 4 for diaganol directions. 


    tempN = currentN->up;
	if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 90;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 90;
				}
			}

		}
	}


    tempN = currentN->down;
    if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 270;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 270;
				}
			}

		}
	}



    tempN = currentN->left_up;
    if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 135;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 135;
				}
			}

		}
	}
	
    tempN = currentN->right_down;
    if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 315;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 315;
				}
			}

		}
	}

    tempN = currentN->left_down;
    if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 215;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 215;
				}
			}

		}
	}
	
    tempN = currentN->left;
    if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 180;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 180;
				}
			}

		}
	}
	
    tempN = currentN->right;
    if( tempN != NULL )
	{
		if(tempN->visited != 1)
		{

			//not on the open list
			if( tempN->open == 0 )
			{
				
				//add to the open list
				open_list[open_index] = tempN;
				tempN->index = open_index;
				open_index++;
				tempN->prev = currentN;
				tempN->cost = tempN->path_cost + currentN->cost;
				tempN->open = 1;
				tempN->rotation = 0;
			}
			else //if( tempN->open == 1 )
			{
				if(  open_list[tempN->index]->cost > (tempN->path_cost + currentN->cost) )
        			{
					open_list[tempN->index]->cost = tempN->path_cost + currentN->cost;
					open_list[tempN->index]->prev = currentN;
					open_list[tempN->index]->rotation = 0;
				}
			}

		}
	}
	}


    //char matrix, used to display the shortest path
    char char_matrix[FIELD_X][FIELD_Y];
    for(i=0;i<FIELD_X;i++)
    {
       	for(j=0;j<FIELD_Y;j++)
 	{
		char_matrix[i][j] = '-';
	}
    }
    



    //dijkstra algorimte has finished, shortest path from any node to origin can be found
    //using the backtracking prev pointer from any node.

    printf("found path:	\n");
    currentN = field_pointers[wanted_x][wanted_y];
    while( currentN != NULL)
    {
        printf("(%d,%d,%d) <- ",currentN->co_x,currentN->co_y,currentN->rotation);
		char_matrix[currentN->co_x][currentN->co_y] = 'x';
		currentN = currentN->prev;
    } 
    char_matrix[wanted_x][wanted_y] = 'X';
    char_matrix[own_x][own_y] = 'O';


    printf("\n\n Graphical path:\n");
    for(i=i_start;i<=i_end;i++)
    {
       	for(j=j_start;j<=j_end;j++)
 	{
		printf(" %c",char_matrix[i][j]);
	}
	printf("\n");

    }

    printf("\n\n input matrix:\n");
    for(i=i_start;i<=i_end;i++)
    {
       	for(j=j_start;j<=j_end;j++)
 	{
		printf("%04d ",field[i][j]);
	}
	printf("\n");

    }

}




